import numpy as np
from algo import *


# 计算u矢量
# 极坐标圆弧和射线
def get_u_orca(p_a, p_b, v_opt=np.zeros(3), circle=np.zeros(3)):
	'''
	求VO的u 转位置坐标系求ORCA的u
	:param v_opt: x,y,z v_opt取 v_a *tau
	:param circle: x,y,r 圆心取p_b + v_b*tau, 半径为r_a + r_b
	:return: u_point,flag
	'''
	# 射线与圆心矢量的夹角
	vc = np.array([circle[0], circle[1], 0])
	c_r = circle[2]
	lvc = norm(p_b - p_a)
	# print('lvc', lvc)
	# print('alpha', alpha * 180 / pi)
	u_point = np.zeros(3, dtype=float)
	u_vec = np.zeros(3, dtype=float)
	lr = np.zeros(3)
	ll = np.zeros(3)
	flag = False
	if lvc < c_r:
		# a进入到VO区域了的处理方法
		print('lvc小于半径c_r 已经与目标重叠')
		return u_vec, u_point, flag, ll, lr

	alpha = asin(c_r / lvc)  # 0-pi

	# print('angle_opt_c', angle_opt_c)
	# v_opt在射线的锐角范围内
	# 圆心矢量右侧r
	# 求右侧射线与圆的交点
	lr_norm = vc * cos(alpha)  # 得到圆心矢量方向 模等于切点矢量模的矢量
	print('alpha', alpha)
	# print('lr_norm', lr_norm)
	lr = trans(point=np.array([lr_norm[0], lr_norm[1], 0, 1]),
	           angle=np.array([0, 0, -alpha]),
	           mode=False)
	# 求左侧射线与圆的交点
	ll = trans(point=np.array([lr_norm[0], lr_norm[1], 0, 1]),
	           angle=np.array([0, 0, alpha]),
	           mode=False)

	# 判断v_opt是否在射线之间 以圆心矢量为0度方向
	angle_opt_c = vector_dot_angle(vc, v_opt)
	if abs(angle_opt_c) <= alpha:

		# 以lr为基准轴 如果(v_opt-vc)到基准的角度小于 (ll-vc)的角度 则在扇形内
		# print('ll, lr', ll, lr)
		print('v_opt在锥形范围内')
		ll_vc = ll[0:3] - vc
		lr_vc = lr[0:3] - vc
		v_opt_vc = v_opt - vc
		# print(vc)
		# print('ll_vc', ll_vc)
		# print('lr_vc', lr_vc)

		a_opt = vector_dot_angle(ll_vc, v_opt_vc)
		a_l = vector_dot_angle(ll_vc, lr_vc)
		print('a_opt', a_opt)
		print('a_l', a_l)
		# 通过叉乘判断v_opt在圆内还是圆外 判断侧 如果在扇形一侧还要满足半径距离约束 排除锥形内扇形外的情况
		if 0 <= a_opt <= a_l:
			print('v_opt在扇形内')
			if norm(v_opt_vc) <= c_r:
				print('v_opt在扇形范围内')
				# print('v_opt_vc norm', norm(v_opt_vc))
				# 求过圆心和v_opt的射线与圆弧的交点就是VO边界上到v_opt最近的点
				u_point = vc + c_r * v_opt_vc / norm(v_opt_vc)
				flag = True

			else:
				print('不在圆内')
				# 在扇形外 锥形内 靠近原点一侧非VO区域
				flag = False
		else:
			print('v_opt在扇形外侧 锥形内 远离原点一侧的VO区域')
			# 求v_opt到射线的垂线
			# 先求圆心矢量与y轴的夹角 y轴[0，1]
			print('在射线域内')
			angle_c = vector_dot_angle(vc, np.array([0, 1, 0]))
			if 0 < angle_opt_c <= alpha:
				# 靠近lr射线 通过极坐标和矢量内积为0得到
				lr_r = v_opt[0] * sin(angle_c + alpha) + v_opt[1] * cos(angle_c + alpha)
				u_point[0] = lr_r * sin(angle_c + alpha)
				u_point[1] = lr_r * cos(angle_c + alpha)
			else:
				# 靠近ll射线
				ll_r = v_opt[0] * sin(angle_c - alpha) + v_opt[1] * cos(angle_c - alpha)
				u_point[0] = ll_r * sin(angle_c - alpha)
				u_point[1] = ll_r * cos(angle_c - alpha)
			flag = True
	else:
		# print('不在锥形内的情况')
		flag = False
	if flag:
		u_vec = u_point - v_opt
	return u_vec, u_point, flag, ll, lr


def get_orca_area(p_a=np.zeros(3), u_point=np.zeros(3), u_vec=np.zeros(3)):
	"""[计算ORCA半平面代数式，转位置坐标系]

	Args:
		p_a ([ndarray]): [a位置]
		u_point ([ndarray]): [u位置]
		u_vec ([ndarray]): [u法向矢量]
	Returns:
		[tuple]: [3*1 直线方程 方向矢量 经过点]
	"""
	eta=1e-5 # 0
	# 已知u
	u_coe = np.zeros(3, dtype=float)
	l_coe = np.zeros(3, dtype=float)

	# 过u点 与u_vec平行直线方程
	if abs(voc_dot(u_vec, np.array([0, 1, 0]))) < eta:
		# 如果u矢量垂直于vx轴
		print('与纵轴平行 本直线B=0')
		u_coe[0] = 1
		u_coe[1] = 0
		u_coe[2] = u_point[0]
	else:
		print('与纵轴不平行 B不为0')
		u_coe[0] = u_vec[1] / u_vec[0]
		u_coe[1] = -1
		u_coe[2] = u_point[1] - u_coe[0] * u_point[0]
	print('u_coe', u_coe)
	# 求过 p_a + u 矢量点的一般方程  这里假设 a b都按ORCA 但实际情况不是 所以不是分摊责任 而是 a负双倍责任
	# point = p_a + 0.5 * u_vec
	point = p_a
	if abs(u_coe[0]) < eta:
		# 如果过u直线与vx平行 那么本直线与lu垂直
		print(' 与vy平行 B为0')
		l_coe[0] = 1
		l_coe[1] = 0
		l_coe[2] = -point[0]
	else:
		# B不为0
		print('B不为0')
		l_coe[0] = -1 / u_coe[0]
		l_coe[1] = -1
		l_coe[2] = point[1] - l_coe[0] * point[0]
	re = [l_coe, u_vec, point]
	return re

def colreg(vec_a, p_b, p_a):
	# 根据某种规则选择稳定的避让方向 避免v在VO区正中央引起的左右跳变
	# 由于get_u_orca进行了是否存在碰撞的判断 这里只需要判断p_b相对p_a的方位角
	# 这里使用航向角 而不是艏向角 作为基准方位
	"""[建议避让方向 左/右侧]

	Args:
		vec_a ([ndarray]): [a的速度矢量]
		p_b ([ndarray]): [b的位置]
		p_a ([ndarray]): [a的位置]

	Returns:
		[int]: [返回0 1 2 对应左 中 右]]
	"""
	ori_angle = vector_dot_angle( pb-pa, vec_a)

	# 规则
	# if abs(ori_angle) < 10*pi/180:
	# 	# 左右15deg范围

	# elif  15*pi/180 <= ab(ori_angle) <  s

	return 0


def boat_constraint(p_a, maxSpeed, v_a, p_goal, tau, dt, constrants):
	'''
	用采样的方法求可行域  找到距离p_goal最近的可行速度
	:param p_a: bot当前位置
	:param v_a: bot当前速度
	:param p_goal: 目标位置
	:param tau: 无碰时间
	:param dt: 单位时间
	:param H: 所有约束
	:return: v_new新的速度方向  sam可行采样点
	'''
	# 生成约束下的采样点集合
	sam = np.zeros(shape=[60, 10, 4])
	# sam_point = np.zeros(shape=[600, 3])
	angle = (np.random.rand(60)*180 - 90)*pi/180 # 左右转角范围 避免急转 rad 30等分
	acc = np.random.rand(10)*maxSpeed - 0.2*maxSpeed# 速度限制 避免速度突变 m/s2 10等分
	flag = True
	min_dis = 1000
	v_new = np.zeros(3)
	for i in range(len(angle)):
		for j in range(len(acc)):
			# 运动约束下采样的位置
			tar_vec = v_a * (acc[j]/norm(v_a))
			# tar_vec = np.array([(v_a[0] + acc[j] * dt), (v_a[1] + acc[j] * dt), 0])
			# if norm(tar_vec) > maxSpeed:
			# 	tar_vec = tar_vec * (maxSpeed/norm(tar_vec))

			tar_vec = trans(point=tar_vec, angle=np.array([ 0, 0, angle[i] ]), mode=False)
			piece = p_a + tau*tar_vec
			
			for sh in constrants:
				flag = False
				# 遍历所有约束 如果满足全部约束 就添加到数组 和v_perf比较夹角
				flag = in_half_plane(sh, piece)
				if flag == False:
					# 不满足某一个约束
					break;
			
			if flag:
				# 如果这个采样点满足所有约束 记录下来画图
				# print('piece is {}'.format(piece))
				sam[i][j][0:3] = piece
				sam[i][j][3] = norm(piece - p_goal)
				
				if sam[i][j][3] < min_dis:
					min_dis = sam[i][j][3]
					v_new[0:3] = tar_vec

	return v_new, sam

def inter_line(my_l, axis_range, num):
	xx = []
	yy = []
	xx = np.linspace(-axis_range, axis_range, num)
	if abs(my_l[1]) < 1e-5:
		# 直线方程B = 0
		xx = np.zeros(num) - l[2]
		xx = np.linspace(-axis_range, axis_range, num)
	else:
		yy = -my_l[0] / my_l[1] * xx + -my_l[2] / my_l[1]
	return xx,yy

# 从一个障碍物测试，直到多个障碍物。
# 补充静态障碍物，线性和圆形
# 测试u的求解效果
test = True
if test:
	lim = 300  # 限制坐标
	lim_v = 3  # 限制速度
	
	r_a = 0.1*lim
	r_b = 0.1*lim
	# a的位置
	pa = np.array([-lim,-lim, 0])
	# 初始速度va
	v_opt_a = np.array([1, 1, 0])

	# b的位置
	pb_all = np.array([[0.6*lim, 0.6*lim, 0]])
	# 速度vb
	v_opt_b_all = np.array([[-2, -2, 0]])

	v_max = 5
	v_perf = np.zeros(3, dtype=float)
	target = np.array([0.8* lim,0.8* lim, 0])

	tau = 20
	count = 100

	t_time = 0
	h_time = 1
	all_time = 1000
	plt.ion()  # 开启绘图
	while t_time < all_time:

		H = []  # 保存所有半平面
		plt.clf()  # 清除上个绘图
		for i in range(len(pb_all)):
			pb = pb_all[i]
			v_opt_b = v_opt_b_all[i]
			v_opt = v_opt_a* tau
			circle = np.array([pb[0] - pa[0], pb[1] - pa[1], r_a+r_b])
			circle += v_opt_b*tau
			# print('circle', circle)
			u_vec, u_point, uflag, ll, lr = get_u_orca(pa, pb, v_opt=v_opt, circle=circle)
			# 返回的结果是相对于p_a的,以p_a为原点的坐标系
			u_point = u_point + pa # a坐标系转大地坐标系
			print('u_vec, u_point, flag is :', u_vec, u_point, uflag)
			plt.plot(u_point[0], u_point[1], 'o')
			if uflag:

				print('速度在障碍区域内')

				f_ll = get_h_line(pa, ll)
				f_lr = get_h_line(pa, lr)
				lx,ly = inter_line(f_ll, lim, count)
				rx,ry = inter_line(f_lr, lim, count)
				plt.plot(lx, ly)
				plt.plot(rx, ry)

				#  画ORCA半平面
				re = get_orca_area(pa, u_point,u_vec )
				l = re[0]
				print('半平面直线方程', l)
				orca = np.array([l[0], l[1], l[2], u_vec[0], u_vec[1], u_vec[2]])  # 半平面直线和方向矢量
				H.append(orca)  # 添加所有的半平面
				x, y = inter_line(l, lim, count)
				plt.plot(x, y, '--')
				print('绘制orca平面')
			print('i', i)

		v_new, record_sim = boat_constraint( pa, v_max, v_opt_a, target, tau, h_time, H)
		v_opt_a = v_new

		t_time += h_time
		# 船更新位置
		pa = pa + h_time * v_new

		x_a, y_a = get_circle([pa[0], pa[1]], r_a)
		plt.plot(x_a, y_a)
		# 障碍物更新位置
		pb_all = pb_all + v_opt_b_all * h_time

		plt.plot(target[0], target[1], 'o')
		print('v_new*tau is ',v_new*tau )
		plt.plot([pa[0], pa[0] + v_new[0] * tau], [pa[1], pa[1] + v_new[1] * tau])

		for ii in range(len(record_sim)):
			for jj in range(len(record_sim[ii])):
				plt.plot(record_sim[ii][jj][0], record_sim[ii][jj][1], 'x')
		

		for pb in pb_all:
			x_b, y_b = get_circle([pb[0], pb[1]], r_b)
			plt.plot(x_b, y_b)
		plt.xlim(-1.2 * lim, 1.2 * lim)
		plt.ylim(-1.2 * lim, 1.2 * lim)

		# 结束循环
		if norm(pa - target) < 6:
			break

		plt.pause(0.1)  # 暂停一秒
		if norm(target - pa) <= 10:
			print('到达目标了')
			break
		plt.ioff()  # 关闭画图的窗口

# 画图测试
# lim = 300
# count = 100
# pa = np.array([-73.3578708 , -73.02176605,   0.        ])
# ll = np.array([66.80337413, 94.69969429,  0.        ])
# lr = np.array([94.96882027, 66.42022334,  0.        ])
# f_ll = get_h_line(pa, ll)
# f_lr = get_h_line(pa, lr)
# lx,ly = inter_line(f_ll, lim, count)
# rx,ry = inter_line(f_lr, lim, count)
# plt.plot(lx, ly)
# plt.plot(rx, ry)
# plt.xlim(-lim, lim)
# plt.ylim(-lim, lim)
# plt.show()
